#define CATCH_CONFIG_MAIN
#include "Navigation.h"
#include <iostream>

struct  Point
{
	float x;
	float y;
	float z;
};

int main(int argc, char * argv[])
{
	Navigation *nav = new Navigation();
	//nav->LoadTile("all_tiles_navmesh.bin");
	if (!nav->LoadTempObstacleTile("E:\\Code\\RecastNavigationAndroidProBuild\\RecastDemo\\Bin\\all_tiles_tilecache.bin"))
	{
		return 0;
	}
	
	float startPos[3] = { 16.7271118, -2.86644363,10.3493652 };
	float endPos[3] = { 24.0440369 ,3.87016678,10.1028423 };

	float midPos[3] = { (startPos[0] + endPos[0])/2, (startPos[1] + endPos[1]) / 2, (startPos[2] + endPos[2]) / 2 };
	float halfExtend[3] = { 1,3,2};

	//float currentPos[3] = { 20.1147003, -2.86784744, 15.3910656 };
	float currentPos2[3] = { -0.823890686, 14.7403679, 12.5349464 };
	//float currentPos1[3] = { -11.4647179, -2.86641121,3.06407166 };
	float currentPos0[3] = { 16.1975670, 9.77402496,-2.67560959 };
	float* currentPos = currentPos0;

	float destPos[3] = { 13.6864204, 8.43191719, 25.2922554 };
	// 	nav->FindNearestPoly(startPos);
	// 	nav->FindPathStraight(startPos, endPos);
	float path_list[30];
	memset(path_list, 0, 3);
	int count = 0;
	nav->FindPathStraight(currentPos, destPos, path_list, &count);
	std::vector<Point> pathsPoint;
	for (int i = 0; i < count - 1; ++i)
	{
		Point p;
		p.x = path_list[i * 3];
		p.y = path_list[i * 3 + 1];
		p.z = path_list[i * 3 + 2];
		pathsPoint.push_back(p);
	}
	for each (Point p in pathsPoint)
	{
		std::cout << "Point.x : " << p.x << ",Point.y : " << p.y << ",Point.z : " << p.z << std::endl;
	}
	pathsPoint.clear();
	NavPoint *navPoint = new NavPoint();
	printf("start pos x:%f,y:%f,z:%f \n", currentPos[0], currentPos[1], currentPos[2]);
	dtStatus s = nav->FindNearestPoly(currentPos, navPoint);
	printf("add obstacle pre status:%d,x:%f,y:%f,z:%f \n", s, navPoint->point[0], navPoint->point[1], navPoint->point[2]);

	int tileIdx = nav->GetTitleIndex(currentPos);
	if (tileIdx != -1)
	{
		const dtMeshTile* meshTile = nav->GetTileByIndexAt(tileIdx);
		//meshTile->
		printf("%d", meshTile->header->layer);
	}
	delete navPoint;
	NavPoint *navPointOut = new NavPoint();
	dtObstacleRef result;
	//nav->AddObstacle(currentPos, 3, 3, &result);
	nav->AddBoxObstacle(startPos,endPos, &result);
	//nav->AddBoxObstacle(midPos, halfExtend,0, &result);
	s = nav->FindNearestPoly(currentPos, navPointOut);
	printf("add obstacle after status:%d,x:%f,y:%f,z:%f\n", s, navPointOut->point[0], navPointOut->point[1], navPointOut->point[2]);
	NavPoint* navPointAroundOut = new NavPoint();
	dtStatus status = nav->FindRandomPointAroundCircle(*navPointOut, 15, navPointAroundOut);
	delete navPointOut;
	bool succ = nav->IsStatusSucceed(status);
	printf("Random location with a radius of 15m .status:%d,x:%f,y:%f,z:%f \n", succ, navPointAroundOut->point[0], navPointAroundOut->point[1], navPointAroundOut->point[2]);
	delete navPointAroundOut;
	float hitPos[3];
	float tangent[3];
	memset(hitPos, 0, sizeof(hitPos));
	memset(tangent, 0, sizeof(tangent));

	nav->FindRaycast(currentPos, destPos, hitPos, tangent);

	pathsPoint.clear();

	nav->FindPathStraight(currentPos, destPos, path_list, &count);
	for (int i = 0; i < count - 1; ++i)
	{
		Point p;
		p.x = path_list[i * 3];
		p.y = path_list[i * 3 + 1];
		p.z = path_list[i * 3 + 2];
		pathsPoint.push_back(p);
	}
	// 	nav->FindNearestPoly(startPos);
		for each (Point p in pathsPoint)
		{
			std::cout << "Point.x : " << p.x << ",Point.y : " << p.y << ",Point.z : " << p.z << std::endl;
		}
		delete nav;
	//getchar();
	return 0;
}